# -*- coding: utf-8 -*-
# 导入系统相关库
import sys
# 导入二进制数据结构处理库
import struct

# 导入数值计算库
import numpy as np
# 导入PyQt6核心模块
from PyQt6.QtCore import Qt, QThread, pyqtSignal, QPointF, QLineF, QRectF,QTimer,QEvent,QDateTime
# 导入PyQt6图形相关类
from PyQt6.QtGui import (QColor, QPainter, QPen, QFont, QPalette, QBrush,QCursor,QMouseEvent)
# 导入PyQt6界面组件
from PyQt6.QtWidgets import (QApplication, QMessageBox,QMainWindow, QDialog,QTableWidget,QTableWidgetItem,
                             QWidget, QVBoxLayout,
                             QHBoxLayout, QComboBox, QPushButton, QSlider,
                             QLabel, QGraphicsView, QGraphicsScene, QFrame,
                             QGraphicsEllipseItem, QGraphicsItemGroup, QGroupBox,
                             QGraphicsSimpleTextItem, QGraphicsItem,QTextEdit,QGraphicsLineItem
                             ,QInputDialog)
# 导入PyQt6串口模块
from PyQt6.QtSerialPort import QSerialPort, QSerialPortInfo
# VS Code风格样式表
VSCODE_STYLE = """
QWidget { font-family:'Segoe UI', Arial; font-size:11pt; color:#cccccc; }
QMainWindow, QDialog { background-color:#1e1e1e; }
QGroupBox { border:1px solid #3c3c3c; border-radius:4px; margin-top:16px; padding-top:12px; }
QGroupBox::title { subcontrol-origin:margin; left:8px; padding:0 4px; color:#bbbbbb; }
QComboBox { background-color:#333333; border:1px solid #3c3c3c; border-radius:3px; padding:4px; min-width:120px; }
QComboBox:hover { border-color:#666666; }
QComboBox::drop-down { width:20px; border-left:1px solid #3c3c3c; }
QPushButton { background-color:#333333; border:1px solid #3c3c3c; border-radius:3px; padding:6px 12px; min-width:80px; }
QPushButton:hover { background-color:#3d3d3d; border-color:#666666; }
QPushButton:pressed { background-color:#2b2b2b; }
QSlider::groove:horizontal { height:4px; background:#333333; border-radius:2px; }
QSlider::sub-page:horizontal { background:#007acc; border-radius:2px; }
QSlider::handle:horizontal { background:#cccccc; border:1px solid #666666; width:12px; margin:-4px 0; border-radius:6px; }
QLabel { color:#bbbbbb; padding:4px 0; }
QGraphicsView { border:0px solid transparent; background:#1e1e1e; }
/* 新增下拉列表样式 */
QComboBox QAbstractItemView {background: #333333;color: #ffffff;
    selection-background-color: #3d3d3d;
    selection-color: #ffffff;
    outline: none;
}
"""
class RadarScene(QGraphicsScene):
    """雷达显示场景，负责绘制背景、网格、坐标轴、距离环和角度引导线"""
    def __init__(self):
        super().__init__()
        # 初始化距离标签组
        self.distance_label_group = QGraphicsItemGroup()
        self.addItem(self.distance_label_group)
        # 网格基本单位和最大显示半径（毫米）
        self.grid_size = 500
        self.max_display_radius = 50000  # 50米
        
        # 缓存上一次可见半径和相关步长
        self._last_visible_radius = 0
        self._cached_main_step = 50     # 主角度步长
        self._cached_sub_step = 10      # 次要角度步长----------------------------
        
        # 设置深灰色背景
        self.setBackgroundBrush(QColor(30, 30, 30))
        
        # 初始化点组并添加到场景
        self.point_group = QGraphicsItemGroup()
        self.addItem(self.point_group)
        # 禁用索引优化，提升动态更新性能
        self.setItemIndexMethod(QGraphicsScene.ItemIndexMethod.NoIndex)

    def drawBackground(self, painter, rect):
        """绘制场景背景（网格、坐标轴、距离环、角度线）"""
        painter.setRenderHint(QPainter.RenderHint.Antialiasing)
        # 填充背景色
        painter.fillRect(rect, self.backgroundBrush().color())
        # 计算有效绘制区域
        draw_rect = rect.intersected(QRectF(
            -self.max_display_radius, -self.max_display_radius,
            self.max_display_radius*2, self.max_display_radius*2
        ))
        # 绘制各层元素
        self._draw_distance_circles(painter)
        self._draw_angle_guides(painter)

    def _draw_distance_circles(self, painter):
        """绘制距离环及标签"""
        
        circle_step = self._calculate_circle_step()
        circle_pen = QPen(QColor(140, 140, 140), 0, Qt.PenStyle.DashLine)
        circle_pen.setCosmetic(True)
        painter.setPen(circle_pen)
        # 清空旧标签
        self.removeItem(self.distance_label_group)
        self.distance_label_group = QGraphicsItemGroup()
        self.addItem(self.distance_label_group)
        
        # 计算最大绘制半径
        visible_radius = self._get_visible_radius()
        max_radius = min(visible_radius + 5000, self.max_display_radius)
        radius = circle_step
        
        # 循环绘制各距离环
        while radius <= max_radius:
            painter.drawEllipse(QPointF(0, 0), radius, radius)
            # 在四个方向添加距离标签
            text = f"{radius}"
            font = QFont('Arial', 8)
            # 四个方向的标签位置和对其方式
            positions = [
                (radius, 0, Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter),
                (0, -radius, Qt.AlignmentFlag.AlignHCenter | Qt.AlignmentFlag.AlignBottom),
                (-radius, 0, Qt.AlignmentFlag.AlignLeft | Qt.AlignmentFlag.AlignVCenter),
                (0, radius, Qt.AlignmentFlag.AlignHCenter | Qt.AlignmentFlag.AlignTop)
            ]
            # 创建并定位标签
            for x, y, alignment in positions:
                text_item = QGraphicsSimpleTextItem(text)
                text_item.setFont(font)
                text_item.setBrush(QColor(140, 140, 140))
                # 标签忽略视图变换（始终保持相同大小）
                text_item.setFlag(QGraphicsItem.GraphicsItemFlag.ItemIgnoresTransformations)
                rect = text_item.boundingRect()
                text_width = rect.width()
                text_height = rect.height()
                # 根据对齐方式计算位置
                if alignment == (Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter):
                    pos_x = x - text_width
                    pos_y = y - text_height/2
                elif alignment == (Qt.AlignmentFlag.AlignHCenter | Qt.AlignmentFlag.AlignBottom):
                    pos_x = x - text_width/2
                    pos_y = y - text_height
                elif alignment == (Qt.AlignmentFlag.AlignLeft | Qt.AlignmentFlag.AlignVCenter):
                    pos_x = x
                    pos_y = y - text_height/2
                elif alignment == (Qt.AlignmentFlag.AlignHCenter | Qt.AlignmentFlag.AlignTop):
                    pos_x = x - text_width/2
                    pos_y = y
                text_item.setPos(pos_x, pos_y)
                self.distance_label_group.addToGroup(text_item)
            radius += circle_step 
            
    def _draw_angle_guides(self, painter):
        """绘制角度引导线"""
        main_step = 10
        # 绘制主角度线
        main_pen = QPen(QColor(140, 140, 140), 0, Qt.PenStyle.DashLine)
        main_pen.setCosmetic(True)
        painter.setPen(main_pen)
        for angle in range(0, 360, main_step):
            self._draw_single_angle_line(painter, angle)

    def _draw_single_angle_line(self, painter, angle):
        """绘制单个角度线"""
        rad = np.deg2rad(angle)  # 角度转弧度
        # 计算线段的终点坐标
        end_x = self.max_display_radius * np.cos(rad)
        end_y = self.max_display_radius * np.sin(rad)
        painter.drawLine(QLineF(0, 0, end_x, end_y))

    def _calculate_circle_step(self):
        """根据可见范围计算距离环步长"""
        visible_radius = self._get_visible_radius()
        if visible_radius > 20000:   # 20米以上：5米间隔
            return 4000
        elif visible_radius > 10000:
            return 2000
        elif visible_radius > 5000: # 10-20米：2米间隔
            return 1000
        elif visible_radius > 2000:  # 2-10米：1米间隔
            return 500
        else:                        # 0.5米以下：0.1米间隔
            return 200

    def _get_visible_radius(self):
        """获取当前视图的可见半径"""
        if not self.views():
            return self.max_display_radius
        # 通过视图的可见区域计算半径
        view = self.views()[0]
        view_rect = view.mapToScene(view.viewport().geometry()).boundingRect()
        return min(view_rect.width(), view_rect.height()) / 2

    def update_points(self, points):
        """更新显示点数据"""
        # 清除旧点
        self.removeItem(self.point_group)
        self.point_group = QGraphicsItemGroup()
        self.addItem(self.point_group)
        # 添加新点
        for point in points:
            self.point_group.addToGroup(point)

class RadarView(QGraphicsView):
    """雷达显示视图，处理视图缩放和导航"""
    def __init__(self):
        super().__init__()
        # 创建场景并设置
        self.scene = RadarScene()
        self.setScene(self.scene)
        # 界面设置
        self.setFrameShape(QFrame.Shape.NoFrame)
        self.setRenderHints(
            QPainter.RenderHint.Antialiasing |
            QPainter.RenderHint.TextAntialiasing
        )
        self.info_label = QGraphicsSimpleTextItem()
        self.info_label.setFlag(QGraphicsItem.GraphicsItemFlag.ItemIgnoresTransformations)
        self.info_label.setBrush(QBrush(Qt.GlobalColor.white))
        self.scene.addItem(self.info_label)
        self.info_label.hide()
        # 视图变换设置

        self.setTransformationAnchor(QGraphicsView.ViewportAnchor.AnchorUnderMouse)
        self.setResizeAnchor(QGraphicsView.ViewportAnchor.AnchorUnderMouse)
        self.setDragMode(QGraphicsView.DragMode.ScrollHandDrag)  # 拖拽滚动模式
        # 初始场景范围
        self.setSceneRect(-20000, -20000, 40000, 40000)
        # 缩放参数
        self.zoom_factor = 1.0
        self.min_zoom = 0.01
        self.max_zoom = 2.0
        # 优化设置
        self.setOptimizationFlags(
            QGraphicsView.OptimizationFlag.DontAdjustForAntialiasing |
            QGraphicsView.OptimizationFlag.DontSavePainterState
        )
        self.viewport().setAttribute(Qt.WidgetAttribute.WA_OpaquePaintEvent)

        # 新增定时器
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.auto_update_info)
        self.timer.start(100)  # 每100ms更新一次

        # 新增距离线
        self.distance_line = QGraphicsLineItem()
        self.distance_line.setPen(QPen(Qt.GlobalColor.red, 3, Qt.PenStyle.DashLine))
        distance_line_pen = self.distance_line.pen()
        distance_line_pen.setCosmetic(True)
              
        self.scene.addItem(self.distance_line)
        self.distance_line.hide()

        # 新增角度线列表
        self.calibration_lines = []
        
        # 创建自定义角度线样式
        self.calibration_pen = QPen(Qt.GlobalColor.green, 2)
        self.calibration_pen.setCosmetic(True)

        # 在初始化代码末尾添加窗口状态跟踪
        self._prev_window_state = Qt.WindowState.WindowNoState
        QApplication.instance().applicationStateChanged.connect(self.handle_app_state)
   
    def add_calibration_line(self):
        """添加新的校准线"""
        if not self.scene:
            return
        
        # 删除旧校准线
        for line in self.calibration_lines:
            self.scene.removeItem(line)
        self.calibration_lines.clear()
        
        # 定义目标角度集合（可在此处修改需要生成的角度值）
        target_angles = [0, 0, 128,228, 209, 90, 198, 295, 288.5, 90]

        #定义目标实际距离合集（可在此处修改需要生成的距离值）
        true_distances = [0, 78, 128,208, 635, 1237, 2823, 5098, 6979, 60000]
        
        main_win = self.main_window()
        
        # 新增：创建数据收集列表
        calibration_data = []

        for idx, angle in enumerate(target_angles):
            # 获取雷达实际距离数据（取前100个历史数据平均值）
            if main_win and hasattr(main_win, 'angle_data_map'):
                nearest_angle = min(main_win.angle_data_map.keys(),
                                 key=lambda a: abs(a - angle))
                # 修复数据结构问题：确保获取的是历史数据列表
                history = main_win.angle_data_map.get(nearest_angle, [])
                # 添加类型检查确保history是可迭代对象
                if isinstance(history, (list, tuple)):
                    recent_history = history[-100:]  # 取最后100个数据
                    actual_distance = sum(recent_history)/len(recent_history) if recent_history else 1000
                else:
                    actual_distance = history  # 兼容旧数据结构
            else:
                actual_distance = 1000  # 默认距离
                
            # 新增：记录校准数据
            test_distance = true_distances[idx]           
            error = actual_distance - test_distance
        # 特殊处理index=1和index=10的情况
            if idx == 0:  # index=1
                actual_distance = 0.0
                error = 0.0
            elif idx+1 == 10:  # index=10
                actual_distance = 60000.0
                error = 0.0
            else:
                error = actual_distance - test_distance
            
            calibration_data.append((
                idx+1, 
                test_distance,
                actual_distance,
                error,
                0.0  # 初始化k值
            ))
        # +++ 新增：误差修正步骤 +++
        # 获取index=2的基准误差值
        base_error = next((item[3] for item in calibration_data if item[0] == 2), 0)
        
        # 对index=2到8的数据进行误差修正
        for i in range(len(calibration_data)):
            index = calibration_data[i][0]
            if 2 <= index <= 9:
                # 创建新元组替换旧数据
                calibration_data[i] = (
                    index,
                    calibration_data[i][1],  # test保持原值
                    calibration_data[i][2],  # actual保持原值
                    calibration_data[i][3] - base_error,  # 误差修正
                    calibration_data[i][4]   # k保持原值
                )

        # 计算斜率k
        for i in range(len(calibration_data)-1):
            current = calibration_data[i]
            next_item = calibration_data[i+1]
            if current[2] != next_item[2]:  # 防止除以0
                k = (current[3] - next_item[3]) / (current[1] - next_item[1])
                calibration_data[i] = (*current[:-1], k)

        result_dialog = QDialog(self)
        result_dialog.setWindowTitle("校准结果")
        result_dialog.setFixedSize(600, 400)  # 新增弹窗尺寸设置
        result_dialog.setStyleSheet("""
            QDialog {
                background-color: #1e1e1e;
                color: #cccccc;
            }
            QTableWidget {
                background-color: #252526;
                gridline-color: #3c3c3c;
                font: 11pt 'Segoe UI';
            }
            QHeaderView::section {
                background-color: #333333;
                padding: 4px;
            }
            QPushButton {
                background-color: #333333;
                border: 1px solid #3c3c3c;
                padding: 6px 12px;
                min-width: 80px;
            }
        """)
        
        # 调整表格列顺序和标题
        table = QTableWidget(len(target_angles), 5)
        table.setHorizontalHeaderLabels(["序号", "测试距离(mm)", "实际距离(mm)", "误差(mm)", "斜率k"])
        table.setColumnWidth(0, 60)
        table.setColumnWidth(1, 120)
        table.setColumnWidth(2, 120)
        table.setColumnWidth(3, 100)
        table.setColumnWidth(4, 100)

        for row, (index, test, actual, err, k) in enumerate(calibration_data):
            table.setItem(row, 0, QTableWidgetItem(str(index)))
            table.setItem(row, 1, QTableWidgetItem(f"{test:.2f}"))
            table.setItem(row, 2, QTableWidgetItem(f"{actual:.2f}"))
            table.setItem(row, 3, QTableWidgetItem(f"{err:.2f}"))
            table.setItem(row, 4, QTableWidgetItem(f"{k:.4f}"))
            
            # 设置特殊行数据
            if index == 1:
                for col in [1, 2, 3, 4]:  # 修复列索引列表
                    table.item(row, col).setText("0.00")
            elif index == 10:  # 改为elif避免重复判断
                table.item(row, 1).setText("60000.00")
                table.item(row, 2).setText("60000.00")
                table.item(row, 3).setText("0.00")
                table.item(row, 4).setText("0.00")
            
            # 设置文本颜色
            for col in range(5):
                item = table.item(row, col)
                item.setForeground(QColor("#cccccc"))
        
        # 设置布局
        layout = QVBoxLayout()
        layout.setContentsMargins(12, 12, 12, 12)  # 增加边距
        layout.addWidget(table)
        
        # 添加按钮行
        button_layout = QHBoxLayout()
        # 添加确认按钮
        btn_confirm = QPushButton("确认")
        btn_confirm.clicked.connect(result_dialog.accept)
        # 添加计算参数按钮
        btn_calculate = QPushButton("计算参数")
        btn_calculate.clicked.connect(lambda: self.calculate_parameters(calibration_data))
        
        button_layout.addWidget(btn_confirm)
        button_layout.addWidget(btn_calculate)
        button_layout.setSpacing(20)  # 设置按钮间距
        layout.addLayout(button_layout)
        
        
        result_dialog.setLayout(layout)
        result_dialog.exec()
    
    def calculate_parameters(self, calibration_data):
        """计算校准参数"""
        import struct
        
        # 提取Distance offset（index=2的误差值）
        distance_offset = next((item[3] for item in calibration_data if item[0] == 2), 0)
        
        # 构建DOLC列表（10组数据）
        dolc_list = []
        for item in calibration_data[:10]:  # 确保只取前10个数据
            index, test, actual, err, k = item
            
            # 处理特殊索引
            if index in (1, 10):
                dist = int(test)
                offset = 0
                coef = 0.0
            else:
                # 转换为要求的格式（Dist单位可能需要调整，根据实际毫米转厘米）
                dist = int(test)  # 假设需要转换为厘米
                offset = int(err)
                coef = k
                
            dolc_list.append((
                dist,       # uint16
                offset,     # int16
                coef        # float32
            ))

        # +++ 新增命令生成部分 +++
        cmd_bytes = bytearray()
        cmd_bytes += struct.pack('<H', 0x4BB4)  # Pack header
        fmem_start = len(cmd_bytes)
        cmd_bytes += struct.pack('<H', 0xFB05)   # FMem header
        cmd_bytes += struct.pack('<H', 0x005A)   # Data size
        cmd_bytes += struct.pack('<h', int(distance_offset))
        cmd_bytes += struct.pack('<H', 10)       # List size
        
        for dist, offset, coef in dolc_list:
            cmd_bytes += struct.pack('<H', dist)
            cmd_bytes += struct.pack('<h', offset)
            cmd_bytes += struct.pack('<f', coef)
            
        cmd_bytes += struct.pack('<H', 0xABCD)  # FMem tail
        
        # 计算XOR校验
        xor_val = 0
        for b in cmd_bytes[fmem_start:-2]:
            xor_val ^= b
        cmd_bytes += bytes([xor_val])
        
        hex_str = ' '.join(f'{b:02X}' for b in cmd_bytes)
        # --- 新增命令生成结束 ---

        # 参数对话框设置
        param_dialog = QDialog(self)
        param_dialog.setWindowTitle("参数详情")
        param_dialog.setFixedSize(500, 450)  # 调整对话框高度
        param_dialog.setStyleSheet("""
            QDialog { background-color: #1e1e1e; color: #cccccc; }
            QTableWidget { 
                background-color: #252526; 
                gridline-color: #3c3c3c; 
                font: 10pt 'Consolas';
            }
            QHeaderView::section { 
                background-color: #333333; 
                padding: 4px;
            }
        """)

        # 创建并填充参数表格（原有功能）
        param_table = QTableWidget(len(dolc_list), 3)
        param_table.setHorizontalHeaderLabels(["距离 (uint16)", "偏移 (int16)", "系数 (float32)"])
        param_table.setColumnWidth(0, 140)
        param_table.setColumnWidth(1, 140)
        param_table.setColumnWidth(2, 200)

        for row, (dist, offset, coef) in enumerate(dolc_list):
            dist_hex = struct.pack('<H', dist).hex().upper().zfill(4)
            offset_hex = struct.pack('<h', offset).hex().upper().zfill(4)
            coef_bytes = struct.pack('<f', coef)
            coef_hex = ' '.join(f"{b:02X}" for b in coef_bytes)
            
            param_table.setItem(row, 0, QTableWidgetItem(dist_hex))
            param_table.setItem(row, 1, QTableWidgetItem(offset_hex))
            param_table.setItem(row, 2, QTableWidgetItem(coef_hex))
            
            for col in range(3):
                item = param_table.item(row, col)
                item.setForeground(QColor("#cccccc"))

        # 新增命令显示控件
        cmd_textedit = QTextEdit()
        cmd_textedit.setPlainText(hex_str)
        cmd_textedit.setStyleSheet("background-color: #252526; color: #D4D4D4;")
        cmd_textedit.setFixedHeight(80)
        
        btn_copy = QPushButton("复制命令")
        btn_copy.clicked.connect(lambda: QApplication.clipboard().setText(hex_str))

        # 组合布局
        layout = QVBoxLayout()
        layout.addWidget(param_table)  # 原有表格
        layout.addWidget(QLabel("\n生成命令:"))  # 新增标签
        layout.addWidget(cmd_textedit)
        layout.addWidget(btn_copy)
        
        param_dialog.setLayout(layout)
        param_dialog.exec()

    class EditableLine(QGraphicsLineItem):
        def __init__(self, angle, distance, pen):
            super().__init__()
            self.angle = angle
            self.initial_distance = distance  # 保留初始距离
            self.current_distance = distance   # 新增当前距离
            self.distance = distance
            self.setPen(pen)
            self.setFlag(QGraphicsItem.GraphicsItemFlag.ItemIsSelectable)
            self.setFlag(QGraphicsItem.GraphicsItemFlag.ItemSendsScenePositionChanges)
                
        def mouseDoubleClickEvent(self, event):
            # 弹出角度输入对话框
            new_angle, ok = QInputDialog.getDouble(
                None, "修改角度", "请输入新角度值:", value=self.angle, decimals=2)
            if ok:
                self.angle = new_angle
                # 新增：获取主窗口的最新距离数据
                radar_view = self.scene().views()[0]  # 获取关联的雷达视图
                main_win = radar_view.main_window()
                if main_win and hasattr(main_win, 'angle_data_map'):
                    nearest_angle = min(main_win.angle_data_map.keys(), 
                                    key=lambda a: abs(a - self.angle))
                    # 修复：获取最新的距离数据
                    distance_data = main_win.angle_data_map[nearest_angle]
                    self.current_distance = distance_data[-1] if isinstance(distance_data, list) else distance_data
                self.update_line_position()
                super().mouseDoubleClickEvent(event)
                    
        def update_line_position(self):
            rad = np.deg2rad(self.angle)
            # 修改：使用current_distance代替固定distance
            end_x = self.current_distance * np.cos(rad)
            end_y = self.current_distance * np.sin(rad)
            self.setLine(0, 0, end_x, end_y)
   
    def auto_update_info(self):
        """定时自动更新信息"""
        # 增强窗口状态检测
        if not self.isVisible() or self.window().windowState() & Qt.WindowState.WindowMinimized:
            return
            
        # 获取当前鼠标位置
        cursor_pos = self.mapFromGlobal(QCursor.pos())
        if not self.viewport().rect().contains(cursor_pos):
            self.info_label.hide()
            return
            
        # 重用鼠标移动事件逻辑
        self.mouseMoveEvent(QMouseEvent(
            QEvent.Type.MouseMove, 
            QPointF(cursor_pos),
            Qt.MouseButton.NoButton,
            Qt.MouseButton.NoButton,  # 使用单个枚举值
            Qt.KeyboardModifier.NoModifier
        ))

    def main_window(self):
        """更可靠地获取主窗口实例"""
        parent = self.parent()
        while parent is not None:
            if isinstance(parent, MainWindow):
                return parent
            parent = parent.parent()
        return None

    def mouseMoveEvent(self, event):
        scene_pos = self.mapToScene(event.pos())
        # 新增：保存最后位置
        self.last_scene_pos = scene_pos  

        x = scene_pos.x()
        y = scene_pos.y()
        
        # 修正1：增加有效距离过滤
        distance = np.hypot(x, y)
        if distance < 100:  # 忽略原点附近的位置
            self.info_label.hide()
            return
            
        # 修正2：调整角度计算精度
        angle = np.degrees(np.arctan2(y, x)) % 360
        query_angle = round(angle, 2)  # 降低精度到2位小数
        
        # 修正3：安全获取数据并转换坐标系
        main_win = self.main_window()
        if main_win:
            # 转换为极坐标系中的毫米单位
            polar_angle = round(angle % 360, 2)
            # 搜索最近的角度（±0.5度范围）
            nearest_angles = [a for a in main_win.angle_data_map.keys() 
                            if abs(a - polar_angle) <= 0.5]
            
            if nearest_angles:
                closest_angle = min(nearest_angles, key=lambda a: abs(a - polar_angle))
                # 修复：获取最新的距离数据（列表最后一个元素）
                distance_data = main_win.angle_data_map[closest_angle]
                current_distance = distance_data[-1] if isinstance(distance_data, list) else distance_data
                # 计算终点坐标（极坐标转笛卡尔坐标）
                end_x = current_distance * np.cos(np.radians(angle))
                end_y = current_distance * np.sin(np.radians(angle))
                
                # 更新红线位置
                self.distance_line.setLine(0, 0, end_x, end_y)
                self.distance_line.show()
                
                status = "有效" if current_distance > 1 else "无效"  # 过滤100mm以下噪声
            else:
                current_distance = 0
                self.distance_line.hide()
                status = "无检测"
        
            self.info_label.setText(
                f"当前角度: {query_angle}°\n"
                f"检测距离: {current_distance}mm\n"  # 使用修正后的变量名
        )
        self.info_label.setPos(scene_pos + QPointF(10, -30))
        self.info_label.show()
        super().mouseMoveEvent(event)

        # 新增：触发单次定时更新
        self.timer.singleShot(100, self.timer.start)
    
    def wheelEvent(self, event):
        """处理鼠标滚轮缩放事件"""
        zoom_factor = 1.15 if event.angleDelta().y() > 0 else 1/1.15
        new_zoom = self.transform().m11() * zoom_factor
        # 限制缩放范围
        if self.min_zoom < new_zoom < self.max_zoom:
            self.scale(zoom_factor, zoom_factor)
        event.accept()

    def showEvent(self, event):
        """窗口显示时初始化视图"""
        self.fitInView(-2000, -2000, 4000, 4000, Qt.AspectRatioMode.KeepAspectRatio)
        self.setVerticalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAlwaysOff)
        self.setHorizontalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAlwaysOff)
        super().showEvent(event)

    def handle_app_state(self, state):
        """处理应用状态变化"""
        if state == Qt.ApplicationState.ApplicationInactive:
            self.timer.stop()
        else:
            if not self.timer.isActive():
                self.timer.start(100)
class SerialWorker(QThread):
    """串口通信工作线程"""
    dataReceived = pyqtSignal(bytes)  # 数据接收信号
    
    def __init__(self, port, baudrate):
        super().__init__()
        self.port = port
        self.baudrate = baudrate
        self.running = True  # 线程运行标志
        self.serial = None   # 串口对象

    def run(self):
        """线程主函数"""
        self.serial = QSerialPort()
        self.serial.setPortName(self.port)
        self.serial.setBaudRate(self.baudrate)
        # 尝试打开串口
        if self.serial.open(QSerialPort.OpenModeFlag.ReadWrite):
            print(f"Connected to {self.port} @ {self.baudrate}")
            # 持续读取数据
            while self.running:
                if self.serial.waitForReadyRead(10):  # 10ms超时
                    data = self.serial.readAll()      # 读取所有可用数据
                    self.dataReceived.emit(data.data())  # 发送原始字节数据
        else:
            print(f"Connection failed: {self.serial.error()}")

    def stop(self):
        """停止线程并关闭串口"""
        self.running = False
        if self.serial:
            if self.serial.isOpen():
                self.serial.close()
                print("Port closed")
            self.serial.deleteLater()
        self.quit()
        self.wait(1000)  # 等待线程结束


class MainWindow(QMainWindow):
    """主窗口类，负责界面和业务逻辑"""
    # CRC校验表
    CRC_TABLE = [
        0x00,0x4d,0x9a,0xd7,0x79,0x34,0xe3,0xae,0xf2,0xbf,0x68,0x25,0x8b,0xc6,0x11,0x5c,
        0xa9,0xe4,0x33,0x7e,0xd0,0x9d,0x4a,0x07,0x5b,0x16,0xc1,0x8c,0x22,0x6f,0xb8,0xf5,
        0x1f,0x52,0x85,0xc8,0x66,0x2b,0xfc,0xb1,0xed,0xa0,0x77,0x3a,0x94,0xd9,0x0e,0x43,
        0xb6,0xfb,0x2c,0x61,0xcf,0x82,0x55,0x18,0x44,0x09,0xde,0x93,0x3d,0x70,0xa7,0xea,
        0x3e,0x73,0xa4,0xe9,0x47,0x0a,0xdd,0x90,0xcc,0x81,0x56,0x1b,0xb5,0xf8,0x2f,0x62,
        0x97,0xda,0x0d,0x40,0xee,0xa3,0x74,0x39,0x65,0x28,0xff,0xb2,0x1c,0x51,0x86,0xcb,
        0x21,0x6c,0xbb,0xf6,0x58,0x15,0xc2,0x8f,0xd3,0x9e,0x49,0x04,0xaa,0xe7,0x30,0x7d,
        0x88,0xc5,0x12,0x5f,0xf1,0xbc,0x6b,0x26,0x7a,0x37,0xe0,0xad,0x03,0x4e,0x99,0xd4,
        0x7c,0x31,0xe6,0xab,0x05,0x48,0x9f,0xd2,0x8e,0xc3,0x14,0x59,0xf7,0xba,0x6d,0x20,
        0xd5,0x98,0x4f,0x02,0xac,0xe1,0x36,0x7b,0x27,0x6a,0xbd,0xf0,0x5e,0x13,0xc4,0x89,
        0x63,0x2e,0xf9,0xb4,0x1a,0x57,0x80,0xcd,0x91,0xdc,0x0b,0x46,0xe8,0xa5,0x72,0x3f,
        0xca,0x87,0x50,0x1d,0xb3,0xfe,0x29,0x64,0x38,0x75,0xa2,0xef,0x41,0x0c,0xdb,0x96,
        0x42,0x0f,0xd8,0x95,0x3b,0x76,0xa1,0xec,0xb0,0xfd,0x2a,0x67,0xc9,0x84,0x53,0x1e,
        0xeb,0xa6,0x71,0x3c,0x92,0xdf,0x08,0x45,0x19,0x54,0x83,0xce,0x60,0x2d,0xfa,0xb7,
        0x5d,0x10,0xc7,0x8a,0x24,0x69,0xbe,0xf3,0xaf,0xe2,0x35,0x78,0xd6,0x9b,0x4c,0x01,
        0xf4,0xb9,0x6e,0x23,0x8d,0xc0,0x17,0x5a,0x06,0x4b,0x9c,0xd1,0x7f,0x32,0xe5,0xa8
    ]

    def __init__(self):
        super().__init__()
        # 数据存储相关
        self.angle_data = []         # 角度历史数据
        self.data_counter = 0        # 数据包计数器
        self.buffer = bytearray()    # 串口数据缓冲区
        self.point_size = 3          # 点显示大小
        self.last_start_angle = 0    # 上一包起始角度
        self.scan_cycle_points = []  # 单次扫描点集合
        self.protocol_combo = 'LH协议'  # 协议选择
        self.protocol = "LH 协议"  # 新增协议属性初始化
        self.angle_data_map = {}  # 角度数据映射
        self.filter_history = {}  # 新增：滤波历史数据存储
        self.filter_window = 5    # 新增：滑动窗口大小
        self.ld_bytes_received = 0
        self.last_ld_stat_time = QDateTime.currentDateTime()
        
        self.init_ui()  # 初始化界面

    def init_ui(self):
        """初始化用户界面"""
        self.setWindowTitle("LiDAR Display")
        self.setGeometry(100, 100, 1200, 800)
        self.setStyleSheet(VSCODE_STYLE)  # 应用样式表

        # 设置调色板
        palette = QPalette()
        palette.setColor(QPalette.ColorRole.Window, QColor(30, 30, 30))
        self.setPalette(palette)

        # 主布局
        main_widget = QWidget()
        main_layout = QHBoxLayout(main_widget)
        main_layout.setContentsMargins(10, 10, 10, 10)
        main_layout.setSpacing(15)

        # 控制面板
        control_panel = QWidget()
        control_panel.setFixedWidth(250)
        control_layout = QVBoxLayout(control_panel)
        control_layout.setContentsMargins(5, 5, 5, 5)
        control_layout.setSpacing(10)

        # 串口设置组
        serial_group = QGroupBox("Connection")
        serial_layout = QVBoxLayout(serial_group)
        self.port_combo = QComboBox()              # 串口选择
        self.baud_combo = QComboBox()              # 波特率选择
        self.baud_combo.addItems(["115200", "230400", "460800",'921600'])
        self.baud_combo.setCurrentIndex(3)         # 默认230400

            # 新增：协议选择
        self.protocol_combo = QComboBox()
        self.protocol_combo.addItems(["LD 协议", "LH 协议"])
        self.protocol_combo.setCurrentIndex(1)     # 默认 'LD 协议'
        self.protocol_combo.currentIndexChanged.connect(self.set_protocol)  # 新增信号连接
        serial_layout.addWidget(QLabel("Protocol:"))  # 标签
        serial_layout.addWidget(self.protocol_combo)  # 添加到布局

        self.connect_btn = QPushButton("Connect")  # 连接按钮
        serial_layout.addWidget(QLabel("Port:"))
        serial_layout.addWidget(self.port_combo)
        serial_layout.addWidget(QLabel("Baudrate:"))
        serial_layout.addWidget(self.baud_combo)
        serial_layout.addWidget(self.connect_btn)

        # 显示设置组
        self.calibrate_btn = QPushButton("校准")
        display_group = QGroupBox("Display")
        display_layout = QVBoxLayout(display_group)
        self.size_slider = QSlider(Qt.Orientation.Horizontal)  # 点大小滑块
        self.size_slider.setRange(1, 10)
        self.size_slider.setValue(3)
        size_label = QLabel("Point Size: 3mm")                # 点大小标签
        self.size_slider.valueChanged.connect(lambda v: size_label.setText(f"Point Size: {v}mm"))
        display_layout.addWidget(QLabel("Point Size:"))
        display_layout.addWidget(self.size_slider)
        display_layout.addWidget(size_label)
        display_layout.addWidget(self.calibrate_btn)
        
                # 新增接收框组
        log_group = QGroupBox("Serial Monitor")
        log_layout = QVBoxLayout(log_group)
        
        # 接收文本框
        self.recv_text = QTextEdit()
        self.recv_text.setReadOnly(True)
        self.recv_text.setFixedHeight(200)
        self.recv_text.setStyleSheet("background-color: #252526; color: #D4D4D4;")
        
        # 控制按钮布局
        btn_layout = QHBoxLayout()
        self.clear_btn = QPushButton("Clear")
        self.save_btn = QPushButton("Save")
        btn_layout.addWidget(self.clear_btn)
        btn_layout.addWidget(self.save_btn)
        
        log_layout.addWidget(self.recv_text)
        log_layout.addLayout(btn_layout)

        # 组装控制面板
        control_layout.addWidget(serial_group)
        control_layout.addWidget(display_group)
        control_layout.addStretch()
        control_layout.addWidget(log_group)
        # 雷达显示视图
        self.radar_view = RadarView()
        
        # 主界面布局
        main_layout.addWidget(control_panel)
        main_layout.addWidget(self.radar_view, 1)
        self.setCentralWidget(main_widget)

        # 连接信号
        self.connect_btn.clicked.connect(self.toggle_connect)  # 连接按钮点击
        self.size_slider.valueChanged.connect(self.set_point_size)  # 点大小改变
        self.calibrate_btn.clicked.connect(self.radar_view.add_calibration_line)
        self.scan_ports()  # 扫描可用串口
    
    def set_protocol(self, index):
        """设置当前协议"""
        self.protocol = self.protocol_combo.currentText()
        print(f"当前协议设置为: {self.protocol}")

    def scan_ports(self):
        """扫描可用串口"""
        self.port_combo.clear()
        for port in QSerialPortInfo.availablePorts():
            self.port_combo.addItem(port.portName())

    def toggle_connect(self):
        """连接/断开串口"""
        if hasattr(self, 'serial_thread') and self.serial_thread.isRunning():
            # 断开连接
            self.serial_thread.stop()
            self.connect_btn.setText("Connect")
        else:
            # 建立连接
            port = self.port_combo.currentText()
            baud = int(self.baud_combo.currentText())
            
            
            self.serial_thread = SerialWorker(port, baud)
            self.serial_thread.dataReceived.connect(self.process_data)
            self.serial_thread.start()
            self.connect_btn.setText("Disconnect")

    def set_point_size(self, size):
        """设置点显示大小"""
        self.point_size = size

    def process_data(self, data):
        """处理接收到的串口数据"""
        self.buffer += data  # 将新数据添加到缓冲区

        # 根据协议选择不同的解析方式
        if self.protocol == "LD 协议":
            self.process_ld_protocol()
        elif self.protocol == "LH 协议":
            self.process_lh_protocol()

    def process_ld_protocol(self):
        """处理 LD 协议的数据"""
        while len(self.buffer) >= 47:
            # 查找包头0x54
            start = self.buffer.find(b'\x54')
            if start == -1:
                self.buffer.clear()
                return
            if start > 0:
                self.buffer = self.buffer[start:]
            if len(self.buffer) < 47:
                return

            packet = bytes(self.buffer[:47])
            if self.check_ld_crc(packet):
                self.parse_ld_packet(packet)
                self.buffer = self.buffer[47:]
            else:
                self.buffer = self.buffer[1:]

    def process_lh_protocol(self):
        """处理 LH 协议的数据"""
        while len(self.buffer) >= 8:  # 至少需要8字节来读取长度
            # 查找包头0xCF, 0xFA
            start = self.buffer.find(b'\xCF\xFA')
            
            if start == -1:
                self.buffer.clear()
                return
            if start > 0:
                self.buffer = self.buffer[start:]
            if len(self.buffer) < 8:
                return

            data_len = struct.unpack('<H', self.buffer[2:4])[0]
            # 计算总包长度
            total_packet_length = 8 + 3 * data_len + 2
            if len(self.buffer) < total_packet_length:
                # 数据不足，等待更多数据
                return

            packet = bytes(self.buffer[:total_packet_length])

            if self.check_lh_crc(packet, data_len):
                self.parse_lh_packet(packet, data_len)
                self.buffer = self.buffer[total_packet_length:]
            else:
                self.buffer = self.buffer[1:]

    def check_ld_crc(self, packet):
        """LD 协议的 CRC 校验"""
        crc = 0
        for byte in packet[:-1]:  # 遍历除CRC字节外的所有字节
            crc = self.CRC_TABLE[(crc ^ byte) & 0xFF]
        return crc == packet[-1]  # 校验结果比对

    def check_lh_crc(self, packet, data_len):
        """LH 协议的 CRC 校验"""
        # LH 协议的校验和计算方式
        # 校验和为起始角度 + 数据点数量 + 跨度 + 所有数据点的强度和距离
        check = 0
        try:
            # 解析起始角度
            start_ang = struct.unpack('<H', packet[4:6])[0]
            
            # 解析跨度
            span = struct.unpack('<H', packet[6:8])[0]
            
            # 计算起始校验和
            check = start_ang + data_len + span

            # 累加所有数据点的强度和距离
            for i in range(8, 8 + 3 * data_len, 3):  # 从第8字节开始，每3字节一个数据点
                en = packet[i]
                di = struct.unpack('<H', packet[i+1:i+3])[0]
                check += en + di
             # 获取接收到的校验和
            received_check = struct.unpack('<H', packet[-2:])[0]
            check = check & 0xFFFF
            return check == received_check
        except Exception as e:
            print(f"Error in LH CRC calculation: {str(e)}")
            return False

    def parse_ld_packet(self, packet):

        """解析数据包"""
        try:
            # 解析包头和长度
            header, data_len = struct.unpack('<BB', packet[0:2])
            # 解析转速和起始角度
            rotation_rate, start_angle = struct.unpack('<HH', packet[2:6])
            # 解析结束角度和时间戳
            end_angle, timestamp = struct.unpack('<HH', packet[42:46])
            
             # 新增字节统计
            '''
            self.ld_bytes_received += 47  # 每个LD协议包固定47字节
            current_time = QDateTime.currentDateTime()
            elapsed_seconds = self.last_ld_stat_time.secsTo(current_time)
            
            if elapsed_seconds >= 1:  # 每秒更新一次统计
                throughput = self.ld_bytes_received / elapsed_seconds
                # 新增时间戳格式化
                time_str = current_time.toString("yyyy-MM-dd hh:mm:ss")
                self.recv_text.append(
                    f"[{time_str}] LD协议吞吐量: {throughput:,.2f} 字节/秒 "  # 添加时间戳
                    f"({self.ld_bytes_received} 总字节)"
                )
                # 重置计数器
                self.ld_bytes_received = 0
                self.last_ld_stat_time = current_time
            '''
            # 计算角度差和步长
            angle_diff = end_angle - start_angle
            if angle_diff < 0:
                angle_diff += 36000
            angle_step = angle_diff / 11.0
            
            # 记录角度信息（用于调试）
            if len(self.angle_data) >= 1:
                prev_end = self.angle_data[-1]['end']
                raw_diff = (start_angle/100 - prev_end)
                seq_diff = ((raw_diff + 180) % 360) - 180  # 计算序列差值
            else:
                seq_diff = float('nan')
            
            self.angle_data.append({
                'start': start_angle / 100,  # 转换为度
                'end': end_angle / 100,
                'diff': angle_diff / 100,
                'step': angle_step / 100,
                'seq_diff': seq_diff
            })
            
            # 限制历史数据长度
            if len(self.angle_data) > 1000:
                self.angle_data.pop(0)
            
            # 解析每个数据点
            for i in range(12):  # 每包12个点
                offset = 6 + i * 3  # 数据点偏移
                distance, intensity = struct.unpack('<HB', packet[offset:offset+3])
                # 计算当前点角度
                current_angle = (start_angle + angle_step * i) % 36000
                angle_deg = round(current_angle * 0.01, 3)  # 保留1位小数
                self.angle_data_map[angle_deg] = distance  # 存储最新距离
                 # 新增：将数据点加入扫描集合（关键修复点）
                self.scan_cycle_points.append((distance, angle_deg, intensity)) 

            # 检测是否开始新的一圈扫描
            if self.is_new_scan(start_angle):
                self.update_display()  # 更新显示
                self.scan_cycle_points.clear()  # 清空当前扫描点
            
            self.last_start_angle = start_angle  # 更新最后起始角度
            
        except Exception as e:
            print(f"Parse error: {str(e)}")
  
    def parse_lh_packet(self, packet, data_len):
        """解析 LH 协议的数据包"""
        try:
            # 1. 检查包头
            header = packet[0:2]
            if header != b'\xCF\xFA':
                print("Invalid LH protocol header")
                return
            # 2. 解析数据长度（已在 process_lh_protocol 中解析）
            # 3. 解析起始角度和跨度
            start_angle = struct.unpack('<H', packet[4:6])[0]
            span = struct.unpack('<H', packet[6:8])[0]
            
            # 检测是否开始新的一圈扫描（当角度回到0度时）
            if start_angle == 0 and self.last_start_angle > 300:
                self.update_display()  # 更新显示
                self.scan_cycle_points.clear()  # 清空当前扫描点
            
            self.last_start_angle = start_angle  # 更新最后起始角度
            
            # 4. 解析数据点
            for i in range(data_len):
                en = packet[8 + 3 * i]
                di_low = packet[8 + 3 * i + 1]
                di_high = packet[8 + 3 * i + 2]
                di = (di_high << 8) | di_low  # 合并高低字节
                angle_deg = round((start_angle + (span / data_len) * i) / 10, 3)

                # 单点离群值检测（快速响应）
                is_outlier = False
                if angle_deg in self.angle_data_map:
                    # 获取历史值
                    if isinstance(self.angle_data_map[angle_deg], list):
                        prev_value = self.angle_data_map[angle_deg][-1]
                    else:
                        prev_value = self.angle_data_map[angle_deg]
                    
                    # 快速离群值检测 - 如果变化超过阈值，认为是离群值
                    if abs(di - prev_value) > 0.1 * prev_value and prev_value > 10:
                        is_outlier = True
                        di = 0  # 使用上一个值替代

                # 应用卡尔曼滤波（比一阶低通滤波响应更快）
                if angle_deg in self.angle_data_map and not is_outlier:
                    # 获取历史值
                    if isinstance(self.angle_data_map[angle_deg], list):
                        prev_value = self.angle_data_map[angle_deg][-1]
                    else:
                        prev_value = self.angle_data_map[angle_deg]
                    
                    # 计算数据变化率
                    change_rate = abs(di - prev_value) / (prev_value + 1)  # 加1避免除零
                    
                    # 动态调整卡尔曼增益K（变化大时增大K值，变化小时减小K值）
                    # K值范围从0.3到0.9，比传统低通滤波响应更快
                    K = min(0.9, max(0.1, 0.1 + change_rate))
                    
                    # 应用卡尔曼滤波公式
                    filtered_value = int(prev_value + K * (di - prev_value))
                    
                    # 更新数据存储
                    if isinstance(self.angle_data_map[angle_deg], list):
                        self.angle_data_map[angle_deg].append(filtered_value)
                        if len(self.angle_data_map[angle_deg]) > 100:
                            self.angle_data_map[angle_deg].pop(0)
                    else:
                        self.angle_data_map[angle_deg] = [self.angle_data_map[angle_deg], filtered_value]
                else:
                    # 首次记录或离群点处理
                    if angle_deg in self.angle_data_map:
                        # 已有记录但是离群点
                        if isinstance(self.angle_data_map[angle_deg], list):
                            self.angle_data_map[angle_deg].append(di)
                            if len(self.angle_data_map[angle_deg]) > 100:
                                self.angle_data_map[angle_deg].pop(0)
                        else:
                            self.angle_data_map[angle_deg] = [self.angle_data_map[angle_deg], di]
                    else:
                        # 首次记录
                        self.angle_data_map[angle_deg] = [di]
                
                # 使用滤波后的值添加到扫描点集合
                filtered_di = self.angle_data_map[angle_deg][-1] if isinstance(self.angle_data_map[angle_deg], list) else self.angle_data_map[angle_deg]
                self.scan_cycle_points.append((filtered_di, angle_deg, en))
                
        except Exception as e:
            print(f"LH Protocol Parse error: {str(e)}")

    def is_new_scan(self, current_start_angle):
        angle_diff = current_start_angle - self.last_start_angle
        return angle_diff < -30000

    def update_display(self):
        points = []
        max_x = max_y = 0
        self.angle_data_map.clear()
        for distance, angle, intensity in self.scan_cycle_points:
            if 0 < distance < 100000:
                rad = np.deg2rad(angle)
                x = distance * np.cos(rad)
                y = distance * np.sin(rad)
                
                # 创建点图形项并设置忽略变换标志
                point = QGraphicsEllipseItem(
                    -self.point_size, -self.point_size,
                    self.point_size*2, self.point_size*2
                )
                point.setFlag(QGraphicsItem.GraphicsItemFlag.ItemIgnoresTransformations)  # 新增
                point.setPos(x, y)
                point.setBrush(QColor.fromHsv(int(intensity * 1) % 360, 255, 200))
                point.setPen(QPen(Qt.PenStyle.NoPen))
                points.append(point)
                self.angle_data_map[round(angle, 3)] = distance
                

        self.radar_view.scene.update_points(points)

if __name__ == '__main__':
    # 创建应用实例
    app = QApplication(sys.argv)
    app.setStyle('Fusion')  # 使用Fusion样式
    # 创建并显示主窗口
    window = MainWindow()
    window.show()
    # 启动事件循环
    sys.exit(app.exec())
